#include <math.h>
#include "PID_v1.h"//PID库
#include "Motion.h"//驱动轮库
#include "SENSOR.h"//传感器库


//编码器引脚
#define pinRA 2//digital 2/int.0 Right
#define pinRB 3//digital 3/int.1 Right
#define pinLA 21//digital 21/int.2 Left
#define pinLB 20//digital 20/int.3 Left

#define unit 9/192//每个中断对应转角

//右编码器
uint8_t stateR = 0;//电平状态记录 A'B'AB
long int cntR = 0;//计数器，前进为正
double angleR = 0;//转角 = cnt * unit
//左编码器
uint8_t stateL = 0;//电平状态记录 A'B'AB
long int cntL = 0;//距离计数器，前进为正
double angleL = 0; //转角 = cnt * unit

//每个状态数值作为数组元素索引，对应距离的变化
const int8_t distanceChange[15] = {0, 1, -1, 0, -1, 0, 0, 1, 1, 0, 0, -1, 0, -1, 1};

extern double ThreeSensorBPNN(double x1[3]);
/*-----------------编码器函数-----------------*/

//初始化编码器
void initEncoder() {

  attachInterrupt(0, encoderIntFunR, CHANGE);
  attachInterrupt(1, encoderIntFunR, CHANGE);
  pinMode(pinRA, INPUT);
  pinMode(pinRB, INPUT);

  attachInterrupt(2, encoderIntFunL, CHANGE);
  attachInterrupt(3, encoderIntFunL, CHANGE);
  pinMode(pinLA, INPUT);
  pinMode(pinLB, INPUT);

  boolean A = digitalRead(pinRA);
  boolean B = digitalRead(pinRB);
  stateR = stateR << 1;
  stateR = stateR & A;
  stateR = stateR << 1;
  stateR = stateR & B;

  A = digitalRead(pinLA);
  B = digitalRead(pinLB);
  stateL = stateL << 1;
  stateL = stateL & A;
  stateL = stateL << 1;
  stateL = stateL & B;
}

//编码器中断函数，右编码器
void encoderIntFunR() {
  boolean A = digitalRead(pinRA);
  boolean B = digitalRead(pinRB);
  stateR = stateR << 1;
  stateR = stateR | A;
  stateR = stateR << 1;
  stateR = stateR | B;
  stateR = stateR & 15; //只保留后四位
  cntR = cntR + distanceChange[stateR];
  angleR = cntR * unit;
}

//编码器中断函数，左编码器
void encoderIntFunL() {
  boolean A = digitalRead(pinLA);
  boolean B = digitalRead(pinLB);
  stateL = stateL << 1;
  stateL = stateL | A;
  stateL = stateL << 1;
  stateL = stateL | B;
  stateL = stateL & 15; //只保留后四位
  cntL = cntL - distanceChange[stateL];//左编码器记录的方向与右编码器恰好相反
  angleL = cntL * unit;
}
/*-----------------编码器函数-----------------*/





//
//int speed = 100;//100°/sec
//int sampleTime = 30;
//
//double setPointR;
////平均相对误差 R 0.00048435 L 0.0037 
//double inputR = 0;
//double outputR = (speed+14.6708)/0.6265;
//
//double setPointL;
//double inputL = 0;
//double outputL = (speed+15.5596)/0.9354;
//
//double KpR = 7.5, KiR = 6, KdR = 0.02;
//double KpL = 0.1, KiL = 5, KdL = 0.02;
//
//
//
//int result1 = 0;
//int result2 = 0;
//
//MOTION Motion(7, 6, 5, 4); //创建驱动轮运动对象
//PID PIDR(&inputR, &outputR, &setPointR, KpR, KiR, KdR, DIRECT);
//PID PIDL(&inputL, &outputL, &setPointL, KpL, KiL, KdL, DIRECT);
//long int count = 0;

SENSOR Sensor(A0,8,9);//创建传感器对象
double netY = 0;//BP神经网络输出
double netX[3] = {0};//三个传感器测量矢量，Infrared,Laser,Ultrasonic

double myTime = 0;
int data=0;

//总初始化
void setup() {
  Serial.begin(9600);//初始化串口通信
  Sensor.init();//传感器初始化
//  Motion.initWheel();   //初始化驱动轮引脚
//  initEncoder();//初始化编码器
//
//  setPointR = speed*sampleTime/1000;//100°/sec
//  setPointL = speed*sampleTime/1000;
//  PIDR.SetMode(AUTOMATIC);//设置PID为自动模式
//  PIDL.SetMode(AUTOMATIC);
//  PIDR.SetSampleTime(sampleTime);//设置PID采样频率为100ms
//  PIDL.SetSampleTime(sampleTime);
//  Serial.print("----setpoint----");
//  Serial.print(setPointR);
//  Serial.print("----");
}



void loop() {
//  count++;
//  Motion.goForward(outputR, outputL);
//
//  //采集
//  inputR = abs(angleR);
//  inputL = abs(angleL);
//
//
//  //计算
//  result1 = PIDR.Compute();
//  result2 = PIDL.Compute();
//
//  //输出
//  if (result1) {
//    Serial.print("Vr/Vl ");
//    Serial.print(inputR);
// Serial.print(" / ");
//    cntR = 0;
//    angleR = 0;
//  }
//  if (result2) {
//    Serial.print(inputL);
//    Serial.print(" c:");
//    Serial.println(count);
//    cntL = 0;
//    angleL = 0;
//  }
//
//if(Serial.available()>0){
//  if(Serial.read()=='H'){
    
//Serial.print("红外线  ");
//Serial.println(Sensor.InfraredRead());
//Serial.print("激光  ");
//Serial.println(Sensor.LaserRead());
//Serial.print("超声波  ");
//Serial.println(Sensor.UltrasonicRead());
//myTime=millis();
//netX[0]=Sensor.InfraredRead();
//Serial.print(millis()-myTime);
//Serial.print("ms  ");
//myTime=millis();
//netX[1]=Sensor.LaserRead();
//Serial.print(millis()-myTime);
//Serial.print("ms  ");
//myTime=millis();
//netX[2]=Sensor.UltrasonicRead();
//Serial.print(millis()-myTime);
//Serial.print("ms  ");
//myTime=millis();
//netY=ThreeSensorBPNN(netX);
//Serial.print(millis()-myTime);
//Serial.print("ms  距离");
//Serial.println(netY);

if(Serial.available()>0){
  data = Serial.read();
//  if(data=='1'){
//    Serial.print("红外线  ");
//    Serial.println(Sensor.InfraredRead());
//  }
//  if(data=='2'){
//    Serial.print("激光  ");
//    Serial.println(Sensor.LaserRead());
//  }
//  if(data=='3'){
//    Serial.print("超声波  ");
//    Serial.println(Sensor.UltrasonicRead());
//  }

if(data=='H'){
  Serial.println(Sensor.InfraredRead());
Serial.println(Sensor.LaserRead());
  Serial.println(Sensor.UltrasonicRead());
  Serial.println("");
}
}

delay(500);



}



double ThreeSensorBPNN(double x1[3])
{
  double a_idx_1;
  double a_idx_2;
  double xp1_idx_0;
  double a;
  int k;
  double b[7];
  static const double b_a[7] = { -1.4939519991350145, 9.84586319123734,
    0.83754468924678183, -3.0564401780385482, -0.16816237999605416,
    6.2791451320940244, 3.525454940574519 };

  static const double c_a[7] = { -0.011483871048306468, -1.7315790548154244,
    -0.049619993209311659, 0.26514582506736167, -1.7761509796049184,
    0.14271491705124842, -0.036210325703600076 };

  static const double d_a[21] = { 2.9443927332522661, -6.5057215063924732,
    -1.4953571511772368, 0.5567950624517084, 0.052097398886085236,
    -0.39804300520066316, -1.700315744566306, -0.43677684905678549,
    -1.999216764176762, -3.2207879153618162, -1.1172557329050361,
    -0.068197623948176356, 9.788020859039456, -4.7053048018845356,
    -1.3938007769152814, -10.402156696812803, -0.35711462497708779,
    -2.0807078020997123, -0.49741917767226074, -2.299596544578796,
    -1.3268793069270708 };

  /*  ===== NEURAL NETWORK CONSTANTS ===== */
  /*  Input 1 */
  /*  Layer 1 */
  /*  Layer 2 */
  /*  Output 1 */
  /*  ===== SIMULATION ======== */
  /*  Dimensions */
  /*  samples */
  /*  Input 1 */
  /*  ===== MODULE FUNCTIONS ======== */
  /*  Map Minimum and Maximum Input Processing Function */
  a_idx_1 = x1[1] * 0.00205128205128205;
  a_idx_2 = (x1[2] - 15.0) * 0.00350877192982456;
  xp1_idx_0 = (x1[0] - 1.0) * 0.00295420974889217 + -1.0;

  /*  Layer 1 */
  /*  Sigmoid Symmetric Transfer Function */
  /*  Layer 2 */
  /*  Output 1 */
  /*  Map Minimum and Maximum Output Reverse-Processing Function */
  a = 0.0;
  for (k = 0; k < 7; k++) {
    b[k] = b_a[k];
    a += c_a[k] * (2.0 / (1.0 + exp(-2.0 * (b[k] + ((d_a[k] * xp1_idx_0 + d_a[k
      + 7] * (a_idx_1 + -1.0)) + d_a[k + 14] * (a_idx_2 + -1.0))))) - 1.0);
  }

  return ((1.5829709720826364 + a) - -1.0) / 0.00203045685279188 + 15.0;
}
